/*
#####################################################################################################
19/11/2012 - UPE - POLI
Codigo de controle do Kit Pendulo Invertido
Aluno: Ricardo Teixeira da Silva

O dispositovo recebe valores descritos abaixo nas Tabelas de Entrada e Saída de Dados,
e retorna dados nos padroes:  
  Dados Originais: posicao, tempo ; angulo, tempo;  estado carro, estado pendulo, line feed.
  Novos Dados: posicao,velocidade posicao, angulo (em graus), velocidade angular
  
versão original por Mércio Andrade (2012)

######################################################################################################

## Tabelas de Entrada e Saída de Dados ##

Se estiver no estado de espera o sistema
pode receber os seguintes comando.

-------------------------------------------
| Comandos de Movimento |	Recebe    |
---------------------------------------
| Parar		        |	0         |
| Mover para Direita    |	1 à 255   |
| Mover para Esquerda   |	-1 à -255 |
------------------------------------------

-------------------------------------------
| Comandos de Controle  |	Recebe    |
-------------------------------------------
| Obter Dados	        |	300       |
| Reiniciar             |	301       |
-------------------------------------------
 
-------------------------------------------
| Comandos de Configurações |	Recebe    |
-------------------------------------------
| Receber Novos Dados       |	302       |
| Receber Dados Originais   |	303       |
| Redefine Valores          |	304       |
| Valores Config            |	305       |
| Enviar dados contínuo     |	306       |
| Enviar dados requisicao   |	307       |
| Ignorar colisões          |	308       |
| Tratar colisões           |	309       |
-------------------------------------------

-------------------------------------------
| Definição das Variáveis   |	Recebe    |
-------------------------------------------
| Pates Disco Codificado    |	xxxxd     | disc
| Pates Fita Codificada     |	xxxxs     | strip
| Limite Esquerdo           |	xxxxl     | left
| Limite Direito            |	xxxxr     | right
-------------------------------------------

* Dados enviados para o controle
-------------------------------------------
| Comandos de Controle  |	Envia     |
-------------------------------------------
| Estado de Espera      |	400       |
| Parada: fim de curso  |	401       |
| Comando Inválido      |	402       |
-------------------------------------------

*/

#define enPin 9 // pino enable CI L293
#define drive_1 10 // pino conectado ao IN1 do CI L293
#define drive_2 11  // pino conectado ao IN2 de CI L293

#define encoder0PinoA 2
#define encoder0PinoB 4
#define encoder1PinoA 3
#define encoder1PinoB 5

volatile int estado_carro = 0; // (1) descolando; (0) parado
volatile int estado_pendulo = 0; // (1) oscilado; (0) parado

char entrada; // armazena o valor recebido pela serial
int val = 0;           // auxiliar entrada
int direcao_carro = -1; // (0) esquerda (1) direita
long velocidade = 0;

// variaveis do encoder int 0 (deslocamento)

volatile long encoder_0_posicao = 0;
volatile long tempo_posicao;
long tempo_inicio_pos=0; 
long tempo_posicao_capturado;

// variaveis do encoder int 1 (angulo)

volatile long encoder_1_angulo = 0;
volatile long tempo_angulo = millis ();
long tempo_inicio_ang=0;

volatile long delta_tempo_angulo = tempo_angulo;

// sensor de fim de curso esquerdo
const int sensorFimCursoEsqPin = 7;
// sensor de fim de curso direito
const int sensorFimCursoDirPin = 6;

int inicializado = 0;

int fimCursoEsq = 0;
int fimCursoDir = 0;
int fimCursoErro = 0;
long timeEnviaErroFimCurso;

const int ledFimcurso = 13;

/////////////////////////////
// Constantes de inicialização

// posicao central
int POSICAO_FINAL = 3600; 
// posicao central
int POSICAO_CENTRAL = POSICAO_FINAL / 2; 
// posicao central
const int VELOCIDADE_CALIBRACAO = 125;

// Limites para o tratamento das colisões
int LIMITE_ESQ = 70; //165;
int LIMITE_DIR = 3530; //3415; 

// estados do sistema
const int INICIALIZANDO = 0;
const int CALIBRANDO = 1;
const int CENTRALIZANDO = 2;
const int ESPERA = 3;

/////////////////////////////

int estadoAtual = INICIALIZANDO;
int fimCurso;

const float pi = 3.1415926535897932384626433832795;

/// Disco codificado
int partesDisco = 2104;
float parteGrau = 360 / partesDisco; 

int tiCentralizar;
int tfCentralizar;
float velocidadeCentralizar;
float velocidadeDeslocamento;
int tiDesloca;
int posicaoAnterior = 0;

// config do padrão de dados a receber (0 = novo, 1  = antigo);
int padraoInfo = 0;

int envioContinuo = 0;
int ignorarFimCurso = 0;
int dir = -1;

void setup() {
  // Configuracao da velocidade de comunicacao serial
  Serial.begin(9600);
  
  // Pinos dos sensores de fim de curso
  pinMode(sensorFimCursoEsqPin, INPUT);
  pinMode(sensorFimCursoDirPin, INPUT);
 
  // Pinos acionamento motor DC
  pinMode(drive_1, OUTPUT); // in1
  pinMode(drive_2, OUTPUT); // in2
   
  // Pinos Interrupcao 0
  pinMode(encoder0PinoA, INPUT); 
  pinMode(encoder0PinoB, INPUT); 
  attachInterrupt(0, doEncoder0, CHANGE ); // sensor conectado a interrupcao 0 
  
  // Pinos iterrupcao 1
  pinMode(encoder1PinoA, INPUT); 
  pinMode(encoder1PinoB, INPUT); 
  attachInterrupt(1, doEncoder1, CHANGE );  // sensor conestado a interrupcao 1 
  
}
 
void loop() {
  
  switch (estadoAtual) {
    
    case INICIALIZANDO:
      fimCurso = digitalRead(sensorFimCursoEsqPin);
       if (fimCurso == HIGH) { 
         estadoAtual = CENTRALIZANDO;
       } else {
         estadoAtual = CALIBRANDO;
       }
      break;
    
    case CALIBRANDO:
      estadoCalibracao();
      break;
    
    case CENTRALIZANDO:
      estadoCentralizacao();
      break;
    
    case ESPERA:
       estadoEspera ();
      if (envioContinuo == 1) {
        tratarComando (300);
      }
      break;
  } 
}

void redefineValoresCentral () {
    encoder_1_angulo = 0;
    encoder_0_posicao = POSICAO_FINAL / 2;
}

// testa o sensor de fim de curso. quando a pino sensorFimCursoEsqPin
// for para nível alto (fimCurso == HIGH).
// quando em nível alto o carro é parado, o encoder_0_posicao vai para zero
// e o estadoAtual vai para CENTRALIZANDO
void estadoCalibracao () { 
  fimCurso = digitalRead(sensorFimCursoEsqPin);
  if (fimCurso == HIGH) {     
    digitalWrite(ledFimcurso, HIGH);  
    desligarCarro ();
    encoder_0_posicao = 0; 
    
    tiCentralizar = millis();    
    
    estadoAtual = CENTRALIZANDO;
  } else {     
    digitalWrite(ledFimcurso, LOW);  
    moverCarro (0, VELOCIDADE_CALIBRACAO);
  }
}

// verifica a posição atual e para quando chegar na POSICAO_CENTRAL
// alterando o estadoAtual para ESPERA. O teste é com >= pois 
// devido ao atrito o carro pode passar um pouco fincando aproximado.
void estadoCentralizacao () { 
  
  if (encoder_0_posicao >= POSICAO_CENTRAL) {
    desligarCarro ();
    estadoAtual = ESPERA;
    
    tfCentralizar = millis();
    
    velocidadeCentralizar = (float)POSICAO_CENTRAL / (float)(tfCentralizar - tiCentralizar);
    
    // envia o código que representa o estado de espera
    Serial.print (400); 
    Serial.write ('\n'); 
    
    redefineValoresCentral ();
    
  } else {
    moverCarro (1, VELOCIDADE_CALIBRACAO); 
  }
}

// retorna true se qualquer dos sensores de fim de curso 
// for acionado caso contrario retorna false
boolean tratarFimCurso () {
  
    fimCursoEsq = digitalRead(sensorFimCursoEsqPin);
    fimCursoDir = digitalRead(sensorFimCursoDirPin);  
    
    return fimCursoEsq || fimCursoDir;
  
}

// Recebendo dados da serial
void estadoEspera () {
  
  if (encoder_0_posicao > LIMITE_ESQ && encoder_0_posicao < LIMITE_DIR) {
    fimCursoErro = 0;
  }
  
  if (tratarFimCurso () && fimCursoErro == 0) {  
    
    if (ignorarFimCurso == 0) {
      desligarCarro (); 
    }
    
    
    // Exceção Parada por fim de curso
    Serial.print (401); 
    Serial.write ('\n');  
    
    fimCursoErro = 1;
    
  }
    
    // checa se existem dados a serem recebidos
    if (Serial.available() > 0) {             
      // Lendo o dado recente 
      entrada = Serial.read();      
      tratarEntrada (); 
    } 
}

void desligarCarro () {
  analogWrite( enPin, 0 ); 
}
  
void moverCarro ( int dir, long vel ) {
	// Determinanndo o sentido de giro do motor DC
	
	if ( velocidade > 255 || velocidade < 0) {
		velocidade = 0;
	}
		
	direcao_carro = dir;
	if(direcao_carro == 0) {
		digitalWrite(drive_1, HIGH);
		digitalWrite(drive_2, LOW); 
		//direcao_carro = 1;        
	} else {
		digitalWrite(drive_1, LOW);
		digitalWrite(drive_2, HIGH);              
	}
	
	analogWrite( enPin, vel ); 
}

// Valores brutos dos sensores e timestamp
void enviarStatusSensores () {
    
    //H,encoder_0_posicao,tempo_posicao,encoder_1_angulo,tempo_angulo,estado_carro,estado_pendulo
   
    Serial.print ("H"); // 'H' indica inicio da mensagem
    Serial.print (","); // Separadador
    Serial.print(encoder_0_posicao); // Deslocamento horizontal
    Serial.print (",");
    Serial.print(tempo_posicao); // Tempo associada à posição
    Serial.print(",");
    Serial.print(encoder_1_angulo); //Deslocamento angular
    Serial.print (",");
    Serial.print(tempo_angulo); // Tempo associado ao angulo deslocado
    Serial.print (",");
    Serial.print (estado_carro); // (0) carro parado, (1) carro em movimento
    Serial.print (",");
    Serial.print (estado_pendulo); // (0) pêndulo Parado, (1) Pêndulo em movimento
    Serial.write("\n"); //insere \n no final da string enviada
    
    // armazenando estaddos anteriores
    //temp1 = velocidade; // armazena entrada anterior do carro    
    estado_pendulo = 0; // seta o estado do pendulo para parado
    estado_carro = 0;  // estado do carro setado para parado
}

void enviarStatusValores () {
	
  int sinal;
  if (encoder_1_angulo < 0) {
    sinal = -1;
  } else {
    sinal = 1;
  }
  
  // DONE deixar o angulo entre -180 e 180
  float r = ((float)encoder_1_angulo / (float)partesDisco);
  float valorAng = (r * 360) - 180; // (r * (180 - (-180))) + (-180)
  
  float parteGrauRad = pi / 180 * parteGrau; // grau pra rad
  float velAngular = parteGrau / delta_tempo_angulo; // grau / (millis / 1000 = seg)
  
  // H,encoder_0_posicao,velocidadeCentralizar,valorAng,velAngular
  
  Serial.print ("H"); // 'H' indica inicio da mensagem
  Serial.print (","); // Separador
  Serial.print(encoder_0_posicao); // Deslocamento horizontal H,3548,26569,0,27387,1,1
  Serial.print (",");
  Serial.print(velocidadeDeslocamento); // Tempo associada a  posição
  Serial.print (",");
  Serial.print(valorAng); // Deslocamento angular
  Serial.print (",");
  Serial.print(velAngular); // Tempo associado ao angulo deslocado  
    
  Serial.write("\n"); //insere "\n" no final da string enviada
  
}

void enviarStatus () {
  
	if (padraoInfo == 1) {
		enviarStatusSensores ();
	} else {
		enviarStatusValores ();
	}
	
}

void enviarConfig () {
        Serial.print ("C");
        Serial.print (",");
        Serial.print(POSICAO_FINAL);
        Serial.print (",");
        Serial.print(partesDisco);
        Serial.print (",");
        Serial.print(LIMITE_ESQ);
        Serial.print (",");
        Serial.print(LIMITE_DIR);  
        Serial.write("\n");  
}

void  tratarEntrada () {
  
	
  	// checa se é ASCII entre 0 e 9	
	if(entrada >= '0' && entrada <= '9') {
		
		converterParaASCII (entrada);
		
	} else if(entrada =='d') { 
		              
		partesDisco = val;
                val = 0;
		          
	} else if(entrada =='s') {
		
		POSICAO_FINAL = val;
                POSICAO_CENTRAL = POSICAO_FINAL / 2;
                val = 0;
                
	} else if(entrada =='l') { 
		              
		LIMITE_ESQ = val;
                val = 0;
		          
	} else if(entrada =='r') {
				  
		LIMITE_DIR = val;
                val = 0;
		
	} else if(entrada =='+') { 
		              
		dir = 1; //direcao_carro = 1;    
		          
	} else if(entrada =='-') {
		
		dir = 0; //direcao_carro = 0;          
		
	} else if (entrada == 10) { // checa se recebeu o caracter newline (\n) (ASCII = 10)
		  
                velocidade = 0;
                
                if (val != 0) {
                
                      if (dir == 0) {
                            direcao_carro = 0;
                      } else {
                            direcao_carro = 1;
                      }
                }
                
                dir = -1;
                		
		// se entrada = \n então novo comando foi lido
		if (val >= 0 && val <= 255) {
  
		      velocidade = val;    	
			
		       if (fimCursoEsq == 1 && direcao_carro == 1) { 
                            moverCarro(direcao_carro, velocidade);
                       } else if (fimCursoDir == 1 && direcao_carro == 0) {
                            moverCarro(direcao_carro, velocidade);
                       } else if (fimCursoEsq == 0 && fimCursoDir == 0) {
                            moverCarro(direcao_carro, velocidade);
                       }
                       
                       velocidadeDeslocamento = (float)(encoder_0_posicao - posicaoAnterior) / (float)(millis() - tiDesloca);
                       
                       posicaoAnterior = encoder_0_posicao;
                       tiDesloca = millis ();
                       
		} else {
			
			tratarComando (val);
			
		}
		
		val = 0; // zera val, para ser aplicada a uma nova sequência de entrada
		
    }
    
    analogWrite(enPin, velocidade );
}

void tratarComando (int comando) {
	
	if (comando == 300) {
		enviarStatus (); // Obter Dados
	} else if (comando == 301) {
                inicializado = 0;
		estadoAtual = CALIBRANDO; // Reiniciar
	} else if (comando == 302) {
		padraoInfo = 0;         
	} else if (comando == 303) {      
		padraoInfo = 1; 
	} else if (comando == 304) {      
		redefineValoresCentral (); 
	} else if (comando == 305) {      
		enviarConfig (); 
	} else if (comando == 306) {      
		envioContinuo = 1;
	} else if (comando == 307) {      
		envioContinuo = 0;
	} else if (comando == 308) {      
		ignorarFimCurso = 1;
	} else if (comando == 309) {      
		ignorarFimCurso = 0;
	} else {
		enviarErro ();
	}
}

void enviarErro () {
	
    // Exceção Comando Inválido
    Serial.print (402); 
    Serial.write ('\n');  
}

void converterParaASCII (char valorLido) {
	val = (val * 10) + (valorLido - '0'); // converte ASCII para valor numérico
}

// Rotina ativada pela interrupcao 0 (deslocamento do carro)
void doEncoder0() {
  
    /* se A==B, entao  deslocamneto eh  para a direita
     * se A!=B, entao o deslocamento e para a esquerda.
     */
    if (digitalRead(encoder0PinoA) == digitalRead(encoder0PinoB)) {    
      tempo_posicao = millis();
      encoder_0_posicao++;
    } else {
      tempo_posicao = millis();
      encoder_0_posicao--;
    }
    estado_carro=1;
  
}

//Rotina ativada pela  interrupcao 1 (variacao do angulo do pendulo)
void doEncoder1() {
  
    long agora = millis();
    delta_tempo_angulo = agora - tempo_angulo;
    tempo_angulo = agora; 
    estado_pendulo = 1;
    
    if (digitalRead(encoder1PinoA) == digitalRead(encoder1PinoB)) {    
      encoder_1_angulo++; 
      if (encoder_1_angulo >= partesDisco) {
         encoder_1_angulo = 0;
      }
     } else {
      encoder_1_angulo--;        
      if (encoder_1_angulo < 0) {
         encoder_1_angulo = partesDisco;
      }
    }
  
}

